/*
  3D - C++ Class Library for 3-D Rigid Transformations
  Copyright (C) 1996, 1997  Gino van den Bergen

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Library General Public
  License as published by the Free Software Foundation; either
  version 2 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Library General Public License for more details.

  You should have received a copy of the GNU Library General Public
  License along with this library; if not, write to the Free
  Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

  Please send remarks, questions and bug reports to gino@win.tue.nl,
  or write to:
                  Gino van den Bergen
		  Department of Mathematics and Computing Science
		  Eindhoven University of Technology
		  P.O. Box 513, 5600 MB Eindhoven, The Netherlands
*/

#ifndef _MATRIX_H_
#define _MATRIX_H_

#include "Quaternion.h"
#include "Vector.h"

class Matrix {
public:
  Matrix() {}
  Matrix(const Quaternion& q) { setValue(q); }
  
  void setValue(const Quaternion& q) {
    Scalar d = q.length2();
    assert(!eqz(d));
    Scalar s = 2 / d;
    Scalar xs = q[X] * s,   ys = q[Y] * s,   zs = q[Z] * s;
    Scalar wx = q[W] * xs,  wy = q[W] * ys,  wz = q[W] * zs;
    Scalar xx = q[X] * xs,  xy = q[X] * ys,  xz = q[X] * zs;
    Scalar yy = q[Y] * ys,  yz = q[Y] * zs,  zz = q[Z] * zs;
    setValue(1 - (yy + zz),       xy + wz,       xz - wy,
	     xy - wz      , 1 - (xx + zz),       yz + wx,
	     xz + wy      , yz - wx      , 1 - (xx + yy));
  }

  const Vector& operator[](int i) const { return *(Vector *)elem[i]; }
  
  Vector& operator[](int i) { return *(Vector *)elem[i]; }

  Scalar rowdot(int i, const Vector& v) const {
    return elem[X][i] * v[X] + elem[Y][i] * v[Y] + elem[Z][i] * v[Z];
  }
  
  void setIdentity() { setValue(1, 0, 0, 0, 1, 0, 0, 0, 1); }
  static Matrix identity() { return Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1); }

  Matrix absolute() const;
  Matrix transpose() const;

  Matrix& operator*=(const Matrix& m);

  friend Matrix operator*(const Matrix& m1, const Matrix& m2);
  friend Matrix multT(const Matrix& m1, const Matrix& m2);
  
protected:
  Matrix(Scalar xx, Scalar xy, Scalar xz, 
	 Scalar yx, Scalar yy, Scalar yz, 
	 Scalar zx, Scalar zy, Scalar zz) {
    setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz);
  }
  
  void setValue(Scalar xx, Scalar xy, Scalar xz, 
		Scalar yx, Scalar yy, Scalar yz, 
		Scalar zx, Scalar zy, Scalar zz) {
    elem[X][X] = xx; elem[X][Y] = xy; elem[X][Z] = xz;
    elem[Y][X] = yx; elem[Y][Y] = yy; elem[Y][Z] = yz;
    elem[Z][X] = zx; elem[Z][Y] = zy; elem[Z][Z] = zz;
  }

private:
  Scalar elem[3][3];
};

ostream& operator<<(ostream& os, const Matrix& m);



inline Matrix Matrix::absolute() const {
   return Matrix(abs(elem[X][X]), abs(elem[X][Y]), abs(elem[X][Z]),
		 abs(elem[Y][X]), abs(elem[Y][Y]), abs(elem[Y][Z]),
		 abs(elem[Z][X]), abs(elem[Z][Y]), abs(elem[Z][Z]));
}

inline Matrix Matrix::transpose() const {
   return Matrix(elem[X][X], elem[Y][X], elem[Z][X],
		 elem[X][Y], elem[Y][Y], elem[Z][Y],
		 elem[X][Z], elem[Y][Z], elem[Z][Z]);
}

inline Matrix& Matrix::operator*=(const Matrix& m) {
  setValue(rowdot(X, m[X]), rowdot(Y, m[X]), rowdot(Z, m[X]),
	   rowdot(X, m[Y]), rowdot(Y, m[Y]), rowdot(Z, m[Y]),
	   rowdot(X, m[Z]), rowdot(Y, m[Z]), rowdot(Z, m[Z]));
  return *this;
}

inline Matrix operator*(const Matrix& m1, const Matrix& m2) {
  return Matrix(m1.rowdot(X, m2[X]), m1.rowdot(Y, m2[X]), m1.rowdot(Z, m2[X]),
		m1.rowdot(X, m2[Y]), m1.rowdot(Y, m2[Y]), m1.rowdot(Z, m2[Y]),
		m1.rowdot(X, m2[Z]), m1.rowdot(Y, m2[Z]), m1.rowdot(Z, m2[Z]));
}

inline Matrix multT(const Matrix& m1, const Matrix& m2) {
  return Matrix(dot(m1[X], m2[X]), dot(m1[Y], m2[X]), dot(m1[Z], m2[X]),
		dot(m1[X], m2[Y]), dot(m1[Y], m2[Y]), dot(m1[Z], m2[Y]),
		dot(m1[X], m2[Z]), dot(m1[Y], m2[Z]), dot(m1[Z], m2[Z]));
}

inline ostream& operator<<(ostream& os, const Matrix& m) {
  return os << 
    m[X][X] << ' ' << m[Y][X] << ' ' << m[Z][X] << endl <<
    m[X][Y] << ' ' << m[Y][Y] << ' ' << m[Z][Y] << endl <<
    m[X][Z] << ' ' << m[Y][Z] << ' ' << m[Z][Z] << endl;
}

#endif


